package com.grt192.sensor.create;

import com.grt192.core.Sensor;
import com.iaroc.irobot.IRobotCreate;
import com.iaroc.irobot.IRobotCreateConstants;

/**
 * Gets IRobotCreate wheel velocities.
 * 
 * @author ajc
 * 
 */
public class EncoderVelocity extends Sensor {

	public static final String LEFT_VELOCITY = "leftVelocity";
	public static final String RIGHT_VELOCITY = "rightVelocity";
	private final IRobotCreate create;

	/**
	 * Make a new VelocityEncoder that wraps both wheels.
	 * 
	 * @param create
	 *            IRobotCreate to poll from
	 * @param pollTime
	 *            time between measurements
	 * @param id
	 *            name of the sensor
	 */
	public EncoderVelocity(IRobotCreate create, int pollTime, String id) {
		this.create = create;
		setSleepTime(pollTime);
		setID(id);
	}

	/**
	 * Polls the IRobotCreate for left wheel velocity, and returns the result.
	 * 
	 * @return [-500mm/s...500mm/s]
	 */
	public double getLeftVelocity() {

		// create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID6);
		create.sensors(IRobotCreateConstants.SENSORS_REQUESTED_LEFT_VELOCITY);
		return create.getRequestedLeftVelocity();
	}

	/**
	 * Polls the IRobotCreate for right wheel velocity, and returns the result.
	 * 
	 * @return [-500mm/s...500mm/s]
	 */
	public double getRightVelocity() {

		// create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID6);
		create.sensors(IRobotCreateConstants.SENSORS_REQUESTED_RIGHT_VELOCITY);
		return create.getRequestedRightVelocity();
	}

	public void poll() {

		create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID6);
		setState(LEFT_VELOCITY, getLeftVelocity());
		setState(RIGHT_VELOCITY, getRightVelocity());

	}

}
